#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>

/*
Dieser Knoten verarbeitet die joy_msg von joy/joy_node und erzeugt daraus eine geometry_msgs::Twist die x y und rotation um die Z achse and den bipedRobin_teleop/biped_teleop knoten weitergibt.
Weiters werden mit den Tasten auf dem Joypad verschiedene Services im bipedRobin_driver/biped_serializer Knoten aufgerufen
Und es kann die Freigabe zum gehen and den bipedRobin_navigation/navigation_master gesendet werden
*/

const double max_speed_x = 0.1; // m/s
const double max_speed_y = 0.05; // m/s
const double max_rotation = 0.2; // rad/s
double vel_x, vel_y, vel_rot; // Geschwindigkeiten

void joyCallback( const sensor_msgs::Joy::ConstPtr& joy_msg )
{
	if( joy_msg->axes.size() < 3 )
	{
		ROS_ERROR( "Too few joystick axes: %d (expected: 3+)", joy_msg->axes.size() );
		return;
	}

	ROS_DEBUG("Received [axis0=%f axis1=%f axis2=%f]", joy_msg->axes[0], joy_msg->axes[1], joy_msg->axes[2] );

	if(joy_msg->buttons[4] == 1){	//L1 als Totmannschalter
		vel_x = joy_msg->axes[1] * max_speed_x;
		vel_y = joy_msg->axes[0] * max_speed_y;
	} else {
		vel_x = 0;
		vel_y = 0;
	}

	if(joy_msg->buttons[5] == 1){ //R1 als Totmannschalter
		vel_rot = joy_msg->axes[2] * max_rotation;
	} else {
		vel_rot = 0;
	}
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "biped_teleop_joy_node");
	ros::NodeHandle n;

	//publisher for cmd_vel and bipedRobin_teleop/biped_teleop
	ros::Publisher  vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
	//subscriber for joy/joy_node
	ros::Subscriber joy_sub = n.subscribe( "joy", 10, &joyCallback );

	ros::Rate loop_rate(100); //publishen der Twist message mit 100 Hz
	while( ros::ok() )
	{
		geometry_msgs::Twist twist_msg;
		twist_msg.linear.x = vel_x;
		twist_msg.linear.y = vel_y;
		twist_msg.angular.z = vel_rot;

		if (1 || vel_x != 0 || vel_y != 0 || vel_rot != 0) {
			vel_pub.publish(twist_msg);
		}

		loop_rate.sleep();
		ros::spinOnce();
	}
}

